/* project4 - ECE 544 Project #4 Application
 *
 * Copyright (c) 2012, 2013, 2014, 2015 Portland State University by Roy Kravitz.  All rights reserved.
 *
 * Author:		Abhishek Malik
 * Date:		05-Mar-2015
 * Revision:	2.1
 *
 * Revision History:
 * -----------------
 * 	21-Feb-2012		RK		Created this program from the xilkernel sample application produced by SDK
 * 	16-May-2013		RK		Updated the program for Nexys3 board and revised homework 2 assignment
 *	12-May-2014		RK		Minor changes to ease support for both Nexys 3 and Nexys 4.  No functional changes
 *  05-Mar-2015		AM		Prepared 1st draft of the XilKernel application for the final project. Updated file name from proj3_starter_app to project4_app
 *  10-Mar-2015		AM		Added all thread functinality
 *  12-Mar-2015		AM		Added functionality for PWM
 *
 * 	Description:
 * 	------------
 *
 *
 * This is a xilkernel application for BLDC Motor Speed Control. There are four GPIOs configured in the hardware
 * for LED and Slide Switches, Motor control, Rotary Encoder and LCD. There are 4 main threads
 * each configured for each of the GPIOs.
 * There is a fit timer in the application which provides an interrupt at every one second. This is used
 * to calculate the speed of the motor.
 * There are 2 axi timers, one is used by xilkernel and the other is used to provide PWM for the speed control.
 *
 */

// Includes for xilkernel
#include "xmk.h"
#include "os_config.h"
#include "config/config_param.h"
#include "sys/ksched.h"
#include "sys/init.h"
#include <pthread.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <semaphore.h>
#include <sys/intr.h>
#include <sys/timer.h>
#include <sys/types.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>

#include "xparameters.h"
#include "platform_config.h"
#include "platform.h"
#include "stdbool.h"
#include "xgpio.h"
#include "xintc.h"
#include "xtmrctr.h"
#include "xstatus.h"
#include "xintc_l.h"
#include "pmod_enc.h"
#include "pmod_clp.h"
#include "pwm_tmrctr.h"

// Declarations
// most of them are just the xparameters renamed
#define SW_LED_GPIO_DEVICEID	XPAR_AXI_GPIO_0_DEVICE_ID
#define ENC_GPIO_DEVICEID		XPAR_AXI_GPIO_1_DEVICE_ID
#define LCD_GPIO_DEVICEID		XPAR_AXI_GPIO_2_DEVICE_ID
#define MOT_GPIO_DEVICEID		XPAR_AXI_GPIO_3_DEVICE_ID
#define INTC_DEVICEID			XPAR_INTC_0_DEVICE_ID
#define WDT_DEVICEID			XPAR_WDTTB_0_DEVICE_ID
#define TMRCTR0_DEVICEID		XPAR_TMRCTR_0_DEVICE_ID
#define INTC_DEVICE_ID			XPAR_INTC_0_DEVICE_ID

#define TMRCTR0_INTR_NUM		XPAR_MICROBLAZE_0_AXI_INTC_AXI_TIMER_0_INTERRUPT_INTR
#define FIT_INTERRUPT_ID		XPAR_MICROBLAZE_0_AXI_INTC_FIT_TIMER_0_INTERRUPT_INTR

#define MICROBLAZE_FREQ			XPAR_MICROBLAZE_CORE_CLOCK_FREQ_HZ

#define PWM_FREQ				500000

// macro functions
#define MIN(a, b)  				( ((a) <= (b)) ? (a) : (b) )
#define MAX(a, b)  				( ((a) >= (b)) ? (a) : (b) )

// GPIO channels for the various GPIO channels used
#define SW_LED_GPIO_INPUT_CHANNEL		1
#define SW_LED_GPIO_OUTPUT_CHANNEL		2
#define LCD_GPIO_OUTPUT_CHANNEL			1
#define	MOT_GPIO_INPUT_CHANNEL			1
#define	MOT_GPIO_OUTPUT_CHANNEL			2

// masks for the GPIO channels
#define	_16_MASK						0xffff
#define _11_MASK						0x7ff
#define _6_MASK							0x3f
#define	_SW_15_MASK						0x8000

// Hall sensor masks
#define _HALL_A_MASK					0x1
#define _HALL_B_MASK					0x2
#define _HALL_C_MASK					0x4

// useful parameters for hall sensors
#define _HALL_B_SH_BITS					1
#define _HALL_C_SH_BITS					2

// Peripheral instances
XGpio SLInst, ENCInst, LCDInst, MOTInst, PWMInst;
XTmrCtr TMRCTR1Inst;
XIntc int_con;

int system_var;

// Message passing variables
typedef struct					// Message definition
{
	int msg_src;
	int msg_value;
} t_LCD_message, *t_LCD_messageptr;

const int lcd_msg_key = 1;		// message key for LCD message queue
struct msqid_ds	lcd_msgstats;	// statistics from message queue
int msg_ID;
int msg_flag = 1;

// variables used ultimately for speed calculation.
int	interrupt_count, old_interrupt_count;

int enc_count=99;

// Synchronization variables
volatile u32 btn_state;			// button state - shared between button handler and button thread
volatile int hall_val;
volatile int enc_val;

// Function declarations
void* master_thread(void *arg);
void* switches_thread(void *arg);
void* rot_enc_thread(void *arg);
void* motor_thread(void *arg);
void* lcd_thread(void *arg);
void  button_handler(void);
void  wdt_handler(void);
XStatus init_peripherals(void);
// clocks for fit timer are 100000000, hence the frequency becomes 1Hz
void FIT_handler(void);
void init_lcd();
void lcd_write(XGpio * InstPtr, int dat);


int main()
{
	XStatus sts;
	xil_printf("inside the main\n\r");
	// initialize the platform and the peripherals
	init_platform();
	sts = init_peripherals();


	if (sts != XST_SUCCESS)
	{
		xil_printf("FATAL ERROR: Could not initialize the peripherals\n\r");
		xil_printf("Please power cycle or reset the system\n\r");
		return -1;
	}

	// Initialize xilkernel
	xilkernel_init();

	// Create the master thread
	xmk_add_static_thread(master_thread, 0);
	msg_ID = msgget(lcd_msg_key,msg_flag);
	// Start the kernel
	xilkernel_start();

	// Should never be reached
	cleanup_platform();

	return 0;
}


// The master thread, this thread doesn't really perform much
void* master_thread(void *arg)
{
	XStatus st;
	pthread_t switches;
	pthread_t rot_encoder;
	pthread_t motor_con;
	pthread_t lcd_con;

	pthread_attr_t attr;
	struct sched_param spar;

	int ret;

	xil_printf("----------------------------------------------------------------------------\r\n");
	xil_printf("ECE 544 Final Project Application \r\n");
	xil_printf("----------------------------------------------------------------------------\r\n");
	xil_printf("This Xilkernel based application reads the rotary encoder value from the PMOD ENC \r\n"
			"which is attached to the FPGA board. It also is used to communnicate with the \r\n"
			"PMOD CLP which is used to display the speed of the motor. It has a separate thread which\r\n"
			"is used to take in the hall sensor inputs from the motor\r\n\r\n");
	xil_printf("----------------------------------------------------------------------------\r\n\r\n\r\n");;

	xil_printf("MASTER: Master Thread Starting\r\n");

	init_lcd(&LCDInst);

	interrupt_count=0;
	// Setting up the initial LCD display
	lcd_write_string(&LCDInst, "Welcome To BLDC", 1, 0);
	lcd_write_string(&LCDInst, "Motor Control", 2, 0);
	sleep(2000);
	// Clearing up the LCD and displaying the static display
	lcd_cls(&LCDInst);
	lcd_write_string(&LCDInst, "Set Pt:", 1, 0);
	lcd_write_string(&LCDInst, "Mot Spd:", 2, 0);
	st = PWM_Initialize(&TMRCTR1Inst, TMRCTR0_DEVICEID, true, MICROBLAZE_FREQ);
	if(st != XST_SUCCESS){
		xil_printf("initialization for axi timer failed\n\r");
	}
	sleep(2000);
	PWM_SetParams(&TMRCTR1Inst, PWM_FREQ, 100);
	PWM_Start(&TMRCTR1Inst);
	//PWM_GetParams(&TMRCTR1Inst,&freq, &duty);

	// set the priority of all but the master thread to 1.  The master thread runs at priority 0
	pthread_attr_init (&attr);
	spar.sched_priority = 1;
	pthread_attr_setschedparam(&attr, &spar);
	// creating a thread for the Slide switches and the leds
	ret = pthread_create (&switches, &attr, (void*) switches_thread, NULL);
	if (ret != 0)
	{
		xil_printf("ERROR (%d) IN MASTER THREAD: could not launch %s\r\n", ret, "switches thread");
		xil_printf("FATAL ERROR: Master Thread Terminating\r\n");
		return (void*) -3;
	}

	// creating a thread for the rotary encoder for reading in from the rotary encoder
	ret = pthread_create (&rot_encoder, &attr, (void*) rot_enc_thread, NULL);
	if (ret != 0)
	{
		xil_printf("ERROR (%d) IN MASTER THREAD: could not launch %s\r\n", ret, "rotary encoder thread thread");
		xil_printf("FATAL ERROR: Master Thread Terminating\r\n");
		return (void*) -3;
	}

	// creating a thread for the motor control
	ret = pthread_create (&motor_con, &attr, (void*) motor_thread, NULL);
	if (ret != 0)
	{
		xil_printf("ERROR (%d) IN MASTER THREAD: could not launch %s\r\n", ret, "hall/motor thread");
		xil_printf("FATAL ERROR: Master Thread Terminating\r\n");
		return (void*) -3;
	}
	// creating a thread for LCD control
	ret = pthread_create (&lcd_con, &attr, (void*) lcd_thread, NULL);
	if (ret != 0)
	{
		xil_printf("ERROR (%d) IN MASTER THREAD: could not launch %s\r\n", ret, "lcd thread");
		xil_printf("FATAL ERROR: Master Thread Terminating\r\n");
		return (void*) -3;
	}

	// enabling the fit interrupt
	enable_interrupt(FIT_INTERRUPT_ID);
	xil_printf("MASTER: Interrupts have been enabled\r\n");

	// master thread main loop
	while(1)
	{
		// in this thread all we do is sleep and let the other threads do their magic
		sleep(500);
	}
	return NULL;
}

/**
 * The rotary encoder thread is used to read in the rotary encoder value using a GPIO instance
 * this is the only thread which can access the rotary encoder gpio value and the functions associated with
 * reading its value (these are the driver functions)
 */
void* rot_enc_thread(void *arg)
{
	// declaring the variables to be used in the thread
	t_LCD_message rot_msg;
	int rot_val;
	int old_rot_val=0;
	int st;
	while(1){
		// calling the GPIO driver function which reads in the GPIO inputs for the
		// rotary encoder and returns a value. The value corresponds to whether the encoder
		// is being moved clock wise or counter clockwise.
		rot_val = read_In_Gpio_ENC(&ENCInst);
		// a return value of 3 indicates that the encoder is being moved clock wise
		if(rot_val==3){
			if(enc_count < MAX_VAL){
				// incrementing the count
				enc_count++;
			}
			else{
				// limiting the max count to 99
				enc_count = MAX_VAL;
			}
		}
		else if(rot_val==4){
			if(enc_count > MIN_VAL){
				// decrementing the count
				enc_count--;
			}
			else{
				// limiting the min count to 0
				enc_count = MIN_VAL;
			}
		}
		// in case of change in the encoder value
		if(enc_count != old_rot_val){
			// we will proceed only in steps of 5
			if(enc_count%5 == 0){
				// source of the messages would be 1 (means coming from rotary encoder thread)
				rot_msg.msg_src = 1;
				rot_msg.msg_value = enc_count;
				// sending the message and making sure it gets through else we display an error on the console
				if(msgsnd(msg_ID, &rot_msg,8,2)==-1){
					xil_printf("lets see the error number in rotary encoder: %s\r\n",strerror(errno));
				}
				// setting the params in the 2nd timer instance which will be used to control
				st = PWM_SetParams(&TMRCTR1Inst, PWM_FREQ, enc_count);
				if(st != XST_SUCCESS){
					xil_printf("pwm set params failed\n\r");
				}
				st = PWM_Start(&TMRCTR1Inst);
				if(st != XST_SUCCESS){
					xil_printf("pwm start failed\n\r");
				}
			}
			old_rot_val = enc_count;
		}
		sleep(1);
	}
	return NULL;
}

/**
 * the motor control thread will read in the hall sensor values and drive the motor and
 * hence form the closed loop and it also increments a variable used for speed count.
 */
void* motor_thread(void *arg)
{
	// local variables used in the thread
	int hall_a, hall_b, hall_c, val_out, old_val=0;
	while(1){
		// reading in the hall sensor values from the gpio and splitting them into 3 separate hall values
		hall_val = XGpio_DiscreteRead(&MOTInst, MOT_GPIO_INPUT_CHANNEL);
		hall_a =(hall_val&_HALL_A_MASK);
		hall_b =(hall_val&_HALL_B_MASK)>>_HALL_B_SH_BITS;
		hall_c =(hall_val&_HALL_C_MASK)>>_HALL_C_SH_BITS;
		// adding a bit of sleep
		sleep(1);
		// following is the motor drive sequence, Rohit and Ravi and Anuja came up with this sequence
		if(hall_a == 0 && hall_b == 1 && hall_c == 0){
			val_out = 0x23;
		}
		else if(hall_a == 0 && hall_b == 1 && hall_c == 1){
			val_out = 0x2c;
		}
		else if(hall_a == 0 && hall_b == 0 && hall_c == 1){
			val_out = 0x0e;
		}
		else if(hall_a == 1 && hall_b == 0 && hall_c == 1){
			val_out = 0x32;
		}
		else if(hall_a == 1 && hall_b == 0 && hall_c == 0){
			val_out = 0x38;
		}
		else if(hall_a == 1 && hall_b == 1 && hall_c == 0){
			val_out = 0x0b;
			// once per this sequence the count would be incremented
			// this count is used to calculate the speed with the help of the FIT timer
			// interrupt
			if(hall_val!=old_val){
				interrupt_count++;
			}
		}
		old_val = hall_val;
		// writing out through the GPIO the driving values
		XGpio_DiscreteWrite(&MOTInst, MOT_GPIO_OUTPUT_CHANNEL, val_out);
	}
	return NULL;
}


/**
 * the primary motivation of having this thread was in case regenrative braking was to be implemented
 * since we never got to that point, regenerative braking implementation wasn't possible. and
 * this thread doesnt get used at all
 */
void* switches_thread(void *arg)
{
	// declaring variables used for the thread
	int sw_15;
	while(1){
		// reading in the switches
		sw_15 = XGpio_DiscreteRead(&SLInst, SW_LED_GPIO_INPUT_CHANNEL);
		// checking the 15th switch and lighting the  15th led if required
		if(sw_15&_SW_15_MASK){
			XGpio_DiscreteWrite(&SLInst, SW_LED_GPIO_OUTPUT_CHANNEL, _SW_15_MASK);
			system_var = 1;
		}
		else{
			XGpio_DiscreteWrite(&SLInst, SW_LED_GPIO_OUTPUT_CHANNEL, 0);
			system_var = 0;
		}
		sleep(1);
	}
	return NULL;
}

/**
 * this is the LCD thread and it is used to display the set point and
 * the speed of the motor on the LCD. It is only this thread which communicates with the
 * LCD which is being written to using a GPIO
 */
void* lcd_thread(void *arg)
{
	// variables to be used by the LCD thread
	t_LCD_message mult_val;
	int msg;
	while(1){
		// receiving a message from the queue
		if(msgrcv(msg_ID,&mult_val, 8, 1,0)==-1){
			xil_printf("An Error has occured while reading the LCD queue: %s\r\n",strerror(errno));
		}
		// if the message source is 1 then that means that it is from the rotary encoder thread
		// and it indicates the set point
		if(mult_val.msg_src == 1){
			msg = mult_val.msg_value;
			// writing to the LCD
			lcd_write_decimal(&LCDInst, msg, 8,1);
		}
		// if the message source is 2 then that means that it is from the fit timer
		// and the message contains the speed
		else if (mult_val.msg_src == 2){
			msg = mult_val.msg_value;
			lcd_write_decimal(&LCDInst, msg, 8,2);
		}
	}
	return NULL;
}


/**
 * this function is used to initialize all the peripherals being used by the embedded system
 */
XStatus init_peripherals(void)
{
	xil_printf("inside init peripherals\n\r");
	XStatus sts;

	// initializing the switches and leds gpio
	sts = XGpio_Initialize(&SLInst,SW_LED_GPIO_DEVICEID);
	if(sts != XST_SUCCESS){
		return XST_FAILURE;
	}
	// setting the output channel to output and the input channel remains unchanged
	XGpio_SetDataDirection(&SLInst,SW_LED_GPIO_OUTPUT_CHANNEL,_16_MASK);

	// initializing the encoder gpio
	sts = XGpio_Initialize(&ENCInst,ENC_GPIO_DEVICEID);
	if(sts != XST_SUCCESS){
		return XST_FAILURE;
	}

	// initializing the LCD gpio
	sts = XGpio_Initialize(&LCDInst,LCD_GPIO_DEVICEID);
	if(sts != XST_SUCCESS){
		return XST_FAILURE;
	}
	XGpio_SetDataDirection(&LCDInst,LCD_GPIO_OUTPUT_CHANNEL,0);

	// initializing the motor gpio
	sts = XGpio_Initialize(&MOTInst,MOT_GPIO_DEVICEID);
	if(sts != XST_SUCCESS){
		return XST_FAILURE;
	}
	XGpio_SetDataDirection(&MOTInst,MOT_GPIO_OUTPUT_CHANNEL,0);

	// initializing the interrupt controller
	sts = XIntc_Initialize(&int_con, INTC_DEVICE_ID);
	if(sts != XST_SUCCESS){
		return XST_FAILURE;
	}

	// connecting the fit timer interrupt to the fit handler function
	sts = XIntc_Connect(&int_con, FIT_INTERRUPT_ID,
			(XInterruptHandler)FIT_handler,
			(void *)0);
	if (sts != XST_SUCCESS)
	{
		return XST_FAILURE;
	}

	return XST_SUCCESS;
}

/**
 * the fit handler function is used to calculate the speed of the motor
 * the fit interrupt goes off in every one second and the counter values is sampled
 * at that time and the speed scaled for calculation
 */
void FIT_handler(void)
{
	t_LCD_message speed_val;
	// disabling the interrupt as we come in
	disable_interrupt(FIT_INTERRUPT_ID);
	if(interrupt_count!=0){
		// perform calculation using the number of interrupts
		// and then pass it to the queue to the LCD
		speed_val.msg_src = 2;
		speed_val.msg_value = interrupt_count*10;
		// checking if the message has been sent from the fit handler correctly or not
		if(msgsnd(msg_ID, &speed_val,8,1)==-1){
			xil_printf("Error while sending the message to the queue from the FIT handler: %s\r\n",strerror(errno));
		}
		interrupt_count =0;
	}

	// acknowledge the interrupt
	acknowledge_interrupt(FIT_INTERRUPT_ID);
	// re enabling the interrupt
	enable_interrupt(FIT_INTERRUPT_ID);
}
